/***************************************************************************************************************************
 * Author: guanty18
 * Update Time: 2021.9.24
***************************************************************************************************************************/
#include <math.h>
#include <ros/ros.h>
#include <std_msgs/UInt8.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <sensor_msgs/Range.h>
#include <uav/servo.h>
#include <uav/position.h>

//*******************************全局变量*******************************


//参数
int i_col;              //是否开启避障
float takeoff_height;   //默认飞行高度
float payload_height;   //降落前稳定高度
float reup_height;      //再起飞初始高度
float camera_x;         
float camera_y;         //图像识别摄像头安装位置
float servo_x[3];
float servo_y[3];       //料仓安装位置
float error[3];         //定位许用误差值

float ABC[8];           //任务目标点默认值1

bool turn_offboard = false;     //是否进入OFFBOARD模式
bool is_land=false;             //是否进入AUTOLAND模式
int command_num=-1;             //任务顺序代号
int find_num=0;                 //图像识别循环计数

//舵机操控信息
uav::servo servo_info;
int servo_id=0;

//无人机飞行目标位置
geometry_msgs::PoseStamped pose;    


//无人机状态变量及其订阅回调
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}


//接收全自主飞行时的启动指令
void start_cb(const std_msgs::UInt8::ConstPtr& msg)
{
    if (msg->data == 1)
    {
        turn_offboard = true;
    }
}


//接收遥控飞行时的启动指令
void rc_cb(const mavros_msgs::RCIn::ConstPtr& rcmsg)
{
	if (rcmsg->channels[7] > 1800) 
    {
		turn_offboard = true;
	} 
    else 
    {
		turn_offboard = false;
	}
}


//tfmini定高雷达的高度值及其订阅回调
float tfmini_z;
void tfmini_cb(const sensor_msgs::Range::ConstPtr& msg)
{
    tfmini_z = msg->range;
}


//无人机当前位置变量及其订阅回调，注意许用误差的大小
int clock_flag=0;           //循环计数
int clock_max=1;            //循环次数上限
bool is_arrived = false;    //标志是否达到误差要求
uav::position position_now;
void pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    double dx = msg->pose.position.x - pose.pose.position.x;
	double dy = msg->pose.position.y - pose.pose.position.y;
    double dz = msg->pose.position.z - pose.pose.position.z;

    //判断是否达到误差要求
	if (abs(dx) < error[0] && abs(dy) < error[1] && abs(dz) < error[2]) {
        clock_flag++;
		if (!is_arrived && clock_flag>=clock_max)
			is_arrived = true;
    }

    //读取当前无人机定位坐标
    position_now.x = msg->pose.position.x;
    position_now.y = msg->pose.position.y;
    position_now.z = msg->pose.position.z;
}


//无人机摄像头目标变量及其订阅回调
float pos_error;
uav::position position_target;
void target_cb(const uav::position::ConstPtr& msg){
    //视野长宽度，与摄像头性能参数有关
    float view_Lx = tfmini_z * 1.46;
    float view_Ly = tfmini_z * 1.08;
    //考虑摄像头位置和料仓位置的校正
    float x_error =- view_Ly*(msg->y) - camera_x + servo_x[servo_id];
    float y_error =- view_Lx*(msg->x) - camera_y + servo_y[servo_id];

    pos_error=(msg->x)*(msg->x)+(msg->y)*(msg->y);
    //目标位置坐标
    position_target.x=position_now.x-x_error;
    position_target.y=position_now.y-y_error;
    position_target.z=position_now.z;
}


//激光雷达识别障碍物及其回调
uav::position position_barrier_tmp;
uav::position position_barrier;
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg){
    float distance_min = msg->ranges[0];
    float angle = msg->angle_min;
    //循环圆周各角度的距离值
    for(int i = 0; i < 720; i++) {
        if(msg->ranges[i] < distance_min){
            angle = (msg->angle_min + msg->angle_increment * i);
            distance_min = msg->ranges[i];
        }
    }
    //计算直角坐标系
    position_barrier_tmp.x=-distance_min*cos(angle);
    position_barrier_tmp.y=-distance_min*sin(angle);
}


//飞行至下一点的函数封装
void fly_to_pose(ros::Publisher local_pos_pub, ros::Rate rate)
{
    //主循环要求：ros程序正常，pos_cb认为没有达到，处于OFFBOARD模式
    while(ros::ok() && !is_arrived && turn_offboard)
    {   
        //发布目标位置指令并保持频率
		local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
        //回显坐标信息
        ROS_INFO("point: %d, \ntarget.xyz: %0.5f/%0.5f/%0.5f, \npose.xyz: %0.5f/%0.5f/%0.5f", command_num,
	         pose.pose.position.x, pose.pose.position.y, pose.pose.position.z,
			position_now.x, position_now.y, position_now.z);
    }
    //重置pos_cb中的变量
    is_arrived=false;
    clock_flag=0;
}

//主飞行任务中，根据图像识别结果校准坐标系
void correct_pos(float Ox, float Oy)
{
    ABC[0]=Ox;
    ABC[1]=Oy;
    ABC[2]=Ox + 1.9;
    ABC[3]=Oy;
    ABC[4]=Ox;
    ABC[5]=Oy - 1.9;
    ABC[6]=Ox + 1.9;
    ABC[7]=Oy - 1.9;
}



//*******************************主函数********************************
int main(int argc, char **argv)
{
    ros::init(argc, argv, "test");
    ros::NodeHandle nh("~");

    //订阅和发布消息
    ros::Subscriber rc_sub = nh.subscribe<mavros_msgs::RCIn>
            ("/mavros/rc/in", 50, rc_cb);
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("/mavros/state", 50, state_cb);
    ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("/mavros/local_position/pose", 50, pos_cb);
    ros::Subscriber laser_sub = nh.subscribe<sensor_msgs::LaserScan>
            ("/scan", 50, scanCallback);
    ros::Subscriber target_sub = nh.subscribe<uav::position>
            ("/uav/target", 50, target_cb);
    ros::Subscriber connected_sub = nh.subscribe<std_msgs::UInt8>
            ("/uav/start", 20, start_cb);
	ros::Subscriber tfmini_sub = nh.subscribe<sensor_msgs::Range>
            ("/mavros/distance_sensor/hrlv_ez4_pub", 20, tfmini_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("/mavros/setpoint_position/local", 20);
    ros::Publisher cmd_vel_pub = nh.advertise<geometry_msgs::Twist>
            ("/mavros/setpoint_velocity/cmd_vel_unstamped", 20);
    ros::Publisher servo_pub = nh.advertise<uav::servo>
            ("/uav/servo", 20);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("/mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("/mavros/set_mode");

    //从yaml文件中读取配置
    nh.param<int>("i_col", i_col, 1);
    nh.param<int>("clock_max", clock_max, 10);
    nh.param<float>("takeoff_height", takeoff_height, 0.8);
    nh.param<float>("payload_height", payload_height, 0.2);
    nh.param<float>("reup_height", reup_height, 0.8);
    nh.param<float>("errorx", error[0], 0.05);
    nh.param<float>("errory", error[1], 0.05);
    nh.param<float>("errorz", error[2], 0.05);
    nh.param<float>("camera_x", camera_x, 0.05);
    nh.param<float>("camera_y", camera_y, 0.0);
    nh.param<float>("servo1x", servo_x[0], 0.0);
    nh.param<float>("servo1y", servo_y[0], 0.0);    
    nh.param<float>("servo2x", servo_x[1], 0.0);
    nh.param<float>("servo2y", servo_y[1], 0.0);    
    nh.param<float>("servo3x", servo_x[2], 0.0);
    nh.param<float>("servo3y", servo_y[2], 0.0);


    //设置ros循环频率
	ros::Rate rate(20);

    //等待确认飞控连接
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }


    // //发布速度控制信息
    // geometry_msgs::Twist cmd_vel;
    // cmd_vel.linear.x = 0.01;
    // cmd_vel.linear.y = 0.01;
    // cmd_vel.linear.z = 0.01;

    // cmd_vel.angular.x = 0;
    // cmd_vel.angular.y = 0;
    // cmd_vel.angular.z = 0;
    // cmd_vel_pub.publish(cmd_vel);


    //设置解锁和模式指令
	mavros_msgs::SetMode set_mode;
	mavros_msgs::CommandBool arm_cmd;
	set_mode.request.custom_mode = "OFFBOARD";
	arm_cmd.request.value = true;
	
    //初始化目标位置值
	pose.pose.position.x = 0;
    pose.pose.position.y = 0;
	pose.pose.position.z = 0;
    


    //*******************************主循环********************************
    //以循环的形式保证始终进入相应模式，实际上内部流程为单向
    ros::Time last_request = ros::Time::now();
    while(ros::ok())
    {
        //确认前提条件为要求OFFBOARD模式：若是，解锁并进入；否则，跳过等待
		if (turn_offboard && !is_land)
        {   
           while( !current_state.armed && ((ros::Time::now() - last_request) > ros::Duration(0.05)))
            {
                if( arming_client.call(arm_cmd) && arm_cmd.response.success)
                {
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }

			while( current_state.mode != "OFFBOARD" && ((ros::Time::now() - last_request) > ros::Duration(0.05)))
            {   
                set_mode.request.custom_mode = "OFFBOARD";
                local_pos_pub.publish(pose);
				if( set_mode_client.call(set_mode) && set_mode.response.mode_sent)
                {
					ROS_INFO("Offboard enabled");
				}
                last_request = ros::Time::now();
            }
        }




        //确认已解锁，然后进入主任务流程
        if( current_state.armed ){

        //根据任务指令序号顺序进入
        switch (command_num) 
        {
			case -1:
                //hold on
				if (current_state.mode == "OFFBOARD" && current_state.armed){
					local_pos_pub.publish(pose);
				}
                correct_pos(position_now.x,position_now.y);

                command_num++;
				break;
            
			case 0:
                //takeoff
				pose.pose.position.x = ABC[0];
                pose.pose.position.y = ABC[1];
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);

                std::cout << "take off!"<<std::endl;
                command_num++;
                break;

            case 1:
                // from O to A (nearly)
                pose.pose.position.x = ABC[2]/2;
                pose.pose.position.y = ABC[3];
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);

		        pose.pose.position.x = ABC[2];
                pose.pose.position.y = ABC[3];
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);

                std::cout << "arrived at A"<<std::endl;
                command_num++;
                break;

            case 2:
                // find A (excatly) and land
                //图像识别循环，确保能够识别圆心
                while(pos_error==0)
                {
                    pose.pose.position.x = ABC[2];
                    pose.pose.position.y = ABC[3];
                    pose.pose.position.z = takeoff_height;
                    fly_to_pose(local_pos_pub, rate);
                    std::cout << "find A ..."<<find_num<<std::endl;

                    ros::spinOnce();       
                    rate.sleep();                 
                }
                //暂时提升定位精度
                error[0]=0.02;
                error[1]=0.02;

                correct_pos(position_target.x-2.0,position_target.y);

                pose.pose.position.x = position_target.x;
                pose.pose.position.y = position_target.y;
                pose.pose.position.z = payload_height;
                fly_to_pose(local_pos_pub, rate);

                //进入AUTOLAND模式自主降落
                set_mode.request.custom_mode = "AUTO.LAND";
                while( current_state.mode != "AUTO.LAND" && ((ros::Time::now() - last_request) > ros::Duration(0.2)))
                {
                    if( set_mode_client.call(set_mode) && set_mode.response.mode_sent )
                    {
                        ROS_INFO("AUTO.LAND");
                    }
                    last_request = ros::Time::now();
                }
                is_land = true;

                std::cout << "pre to payload at A"<<std::endl;
                command_num++;
                break;

            case 3:
                // payload at A
                //暂停等待，然后退出land状态
                for(int clock=0;clock<=60;clock++)
                {
					rate.sleep();
                }
				is_land =false;
                                
                //恢复定位精度
			    error[0]=0.05;
                error[1]=0.05;
                
                //发布投料指令
				servo_info.id = 33;
                servo_info.direction = 1;
                servo_pub.publish(servo_info);
                std::cout << "payload at A!"<<std::endl;
                servo_id++;

                command_num++;
                break;

            case 4:
                // re-up at A
                pose.pose.position.x = position_now.x;
                pose.pose.position.y = position_now.y;
                pose.pose.position.z = reup_height;
                fly_to_pose(local_pos_pub, rate);

                pose.pose.position.x = position_now.x;
                pose.pose.position.y = position_now.y;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "re-takeoff at A!"<<std::endl;

                // from A to barrier1
                pose.pose.position.x = position_now.x-0.4;
                pose.pose.position.y = position_now.y-0.4;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "approach barrier 1 !"<<std::endl;

                command_num++;
                break;

            case 5:
                // collision avoidance 1 
                //完成避障的一系列动作
				if(i_col)
				{            	
				position_barrier = position_barrier_tmp;

                pose.pose.position.x = position_now.x+position_barrier.x+0.6;
                pose.pose.position.y = position_now.y+position_barrier.y+0.6;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                
                position_barrier = position_barrier_tmp;

                pose.pose.position.x = position_now.x;
                pose.pose.position.y = position_now.y;
                pose.pose.position.z = takeoff_height + 0.6;
                fly_to_pose(local_pos_pub, rate);
		
		        pose.pose.position.z = takeoff_height + 1.1;
		        fly_to_pose(local_pos_pub, rate);


                pose.pose.position.x = position_now.x+position_barrier.x*2-0.4;
                pose.pose.position.y = position_now.y+position_barrier.y*2-0.4;
                pose.pose.position.z = takeoff_height + 1.1;
                fly_to_pose(local_pos_pub, rate);



		        pose.pose.position.z = takeoff_height + 0.6;
		        fly_to_pose(local_pos_pub, rate);
                
                pose.pose.position.x = position_now.x;
                pose.pose.position.y = position_now.y;
                pose.pose.position.z = takeoff_height;    
                fly_to_pose(local_pos_pub, rate);
              	}  

                std::cout << "collision avoidance 1 !"<<std::endl;
               
                command_num++;
                break;

            case 6:
                // from barrier1 to B
                pose.pose.position.x = ABC[4];
                pose.pose.position.y = ABC[5];
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "arrived at B"<<std::endl;
                command_num++;
                break;

            case 7:
                // find B (excatly)
                while(pos_error==0)
                {
                    pose.pose.position.x = ABC[4];
                    pose.pose.position.y = ABC[5];
                    pose.pose.position.z = takeoff_height;
                    fly_to_pose(local_pos_pub, rate);
                    std::cout << "find B ..."<<find_num<<std::endl;

                    ros::spinOnce();       
                    rate.sleep();                 
                }

                error[0]=0.02;
                error[1]=0.02;
                correct_pos(position_target.x,position_target.y+1.9);

                pose.pose.position.x = position_target.x;
                pose.pose.position.y = position_target.y;
                pose.pose.position.z = payload_height;
                fly_to_pose(local_pos_pub, rate);

                set_mode.request.custom_mode = "AUTO.LAND";
                while( current_state.mode != "AUTO.LAND" && ((ros::Time::now() - last_request) > ros::Duration(0.2)))
                {
                    if( set_mode_client.call(set_mode) && set_mode.response.mode_sent )
                    {
                        ROS_INFO("AUTO.LAND");
                    }
                    last_request = ros::Time::now();
                }
                is_land = true;

                std::cout << "pre to payload at B"<<std::endl;
                
                command_num++;
                break;

            case 8:
                // payload at B
     
                servo_info.id = 35;
                servo_info.direction = 1;
                servo_pub.publish(servo_info);
                std::cout << "payload at B!"<<std::endl;
                servo_id++;

                for(int clock=0;clock<=60;clock++)
                {
                    rate.sleep();
                }
                is_land = false;

                error[0]=0.05;
                error[1]=0.05;


            
                command_num++;
                break;

            case 9:
                // re-up at B
                pose.pose.position.x = position_now.x; 
                pose.pose.position.y = position_now.y;
                pose.pose.position.z = reup_height;
                fly_to_pose(local_pos_pub, rate);

                pose.pose.position.x = position_now.x; 
                pose.pose.position.y = position_now.y;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "re-takeoff at B!"<<std::endl;


                // collision avoidance 2 

                pose.pose.position.x = position_now.x+0.3;
                pose.pose.position.y = position_now.y;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);

                if(i_col)
				{

                position_barrier = position_barrier_tmp;
                
                pose.pose.position.x = position_now.x+position_barrier.x-0.5;
                pose.pose.position.y = position_now.y+position_barrier.y;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);

                position_barrier = position_barrier_tmp;

                pose.pose.position.x = position_now.x;
                pose.pose.position.y = position_now.y;
                pose.pose.position.z = takeoff_height + 0.6;
                fly_to_pose(local_pos_pub, rate);

                pose.pose.position.z = takeoff_height + 1.2;
                fly_to_pose(local_pos_pub, rate);

                pose.pose.position.x = position_now.x+position_barrier.x*2+0.1;
                pose.pose.position.y = position_now.y+position_barrier.y*2;
                pose.pose.position.z = takeoff_height + 1.2;
                fly_to_pose(local_pos_pub, rate);


                pose.pose.position.z = takeoff_height + 0.6;
                fly_to_pose(local_pos_pub, rate);
                
                pose.pose.position.x = position_now.x;
                pose.pose.position.y = position_now.y;
                pose.pose.position.z = takeoff_height;    
                fly_to_pose(local_pos_pub, rate);

               
                std::cout << "collision avoidance 2 !"<<std::endl;
                }
                command_num++;
                break;
            
            case 10:
                // from barrier2 to C
		        pose.pose.position.x = ABC[6];
                pose.pose.position.y = ABC[7];
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "arrived at C"<<std::endl;
                command_num++;
                break;

            case 11:
                // find C (excatly)
                while(pos_error==0)
                {
                    pose.pose.position.x = ABC[6];
                    pose.pose.position.y = ABC[7];
                    pose.pose.position.z = takeoff_height;
                    fly_to_pose(local_pos_pub, rate);
                    std::cout << "find C ..."<<find_num<<std::endl;
                    ros::spinOnce();       
                    rate.sleep();                 
                }

                error[0]=0.02;
                error[1]=0.02;
                correct_pos(position_target.x-1.9,position_target.y+1.9);

                pose.pose.position.x = position_target.x;
                pose.pose.position.y = position_target.y;
                pose.pose.position.z = payload_height;
                fly_to_pose(local_pos_pub, rate);

                set_mode.request.custom_mode = "AUTO.LAND";
                while( current_state.mode != "AUTO.LAND" && ((ros::Time::now() - last_request) > ros::Duration(0.2)))
                {
                    if( set_mode_client.call(set_mode) && set_mode.response.mode_sent )
                    {
                        ROS_INFO("AUTO.LAND");
                    }
                    last_request = ros::Time::now();
                }
                is_land = true;

                std::cout << "pre to payload at C"<<std::endl;
                
                command_num++;
                break;

            case 12:
                // payload at C

                servo_info.id = 37;
                servo_info.direction = 1;
                servo_pub.publish(servo_info);
                std::cout << "payload at C!"<<std::endl;
                servo_id++;


                for(int clock=0;clock<=60;clock++)
                {
                    rate.sleep();
                }
                is_land = false;

                error[0]=0.05;
                error[1]=0.05;


                
                command_num++;
                break;

            case 13:
                // re-up at C
                pose.pose.position.x = position_now.x; 
                pose.pose.position.y = position_now.y;
                pose.pose.position.z = reup_height;
                fly_to_pose(local_pos_pub, rate);

                pose.pose.position.x = position_now.x; 
                pose.pose.position.y = position_now.y;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "re-takeoff at C!"<<std::endl;


                // back to O
                pose.pose.position.x = ABC[2];
                pose.pose.position.y = ABC[3]-1;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);

                pose.pose.position.x = ABC[2];
                pose.pose.position.y = ABC[3];
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);

                pose.pose.position.x = ABC[2]-1;
                pose.pose.position.y = ABC[3];
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);

                pose.pose.position.x = ABC[0];
                pose.pose.position.y = ABC[1];
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                command_num++;
                break;

            case 14:    
                // find O (excatly)

                while(pos_error==0)
                {
                    pose.pose.position.x = ABC[0];
                    pose.pose.position.y = ABC[1];
                    pose.pose.position.z = takeoff_height;
                    fly_to_pose(local_pos_pub, rate);
                    std::cout << "find O ..."<<find_num<<std::endl;

                    ros::spinOnce();       
                    rate.sleep();                 
                }


                pose.pose.position.x = position_target.x;
                pose.pose.position.y = position_target.y;
                pose.pose.position.z = payload_height;
                fly_to_pose(local_pos_pub, rate);
            
                command_num++;
                break;

			
			case 15:
                set_mode.request.custom_mode = "AUTO.LAND";
                last_request = ros::Time::now();
                while( current_state.mode != "AUTO.LAND" && ((ros::Time::now() - last_request) > ros::Duration(2.0)))
                {
                     if( set_mode_client.call(set_mode) && set_mode.response.mode_sent)
                     {
                         std::cout << "AUTO.land"<<std::endl;
                     }
                }
                is_land=true;
               // 上锁
                last_request = ros::Time::now();
                arm_cmd.request.value = false;
                if( current_state.armed)
                {
                    if( arming_client.call(arm_cmd) &&arm_cmd.response.success)
                     {
                         std::cout << "Vehicle disarmed"<<std::endl;
                     }
                     last_request = ros::Time::now();
                 }
                break;
        }

        }
        

        ros::spinOnce();
        rate.sleep();
    }
    return 0;

}


